#include "physics_simulator.h"
#include "application/map/map_application_service.h"
#include <algorithm>
#include <cmath>
#include <QString>

namespace application {
namespace simulation {

PhysicsSimulator::PhysicsSimulator(
    std::shared_ptr<MapApplicationService> mapService,
    std::shared_ptr<kernel::ILogger> logger)
    : m_mapService(mapService)
    , m_logger(logger)
    , m_maxSpeed(0.8)           // 默认最大速度 0.8m/s
    , m_arrivalThreshold(0.05)  // 默认到站阈值 0.05m
{
}

void PhysicsSimulator::updateVehicle(int agvNumber, double deltaTime) {
    auto* state = getVehicleState(agvNumber);
    if (!state) {
        return;
    }

    const bool paused = state->isPaused;
    if (paused) {
        // 暂停时保持静止
        state->velocity = 0.0;
        state->acceleration = 0.0;
    }

    // 如果没有路径，直接返回
    const bool hasPath = !state->pathStations.empty() &&
        state->currentSegmentIndex < state->pathStations.size();

    if (!hasPath) {
        state->velocity = 0.0;
        state->acceleration = 0.0;
    } else if (!paused) {
        // 获取当前目标站点
        const auto& targetStationId = state->pathStations[state->currentSegmentIndex];
        QString targetStation = QString::fromStdString(targetStationId);
        QPointF targetPos = getStationPosition(targetStation, state->currentMap);

        // 计算到目标的距离和方向
        QPointF direction = targetPos - state->position;
        double distance = std::sqrt(direction.x() * direction.x() + direction.y() * direction.y());

        // 到站判断
        if (distance < m_arrivalThreshold) {
            // 到达当前站点
            state->position = targetPos;
            state->currentSegmentIndex++;

            if (m_logger) {
                const QString msg = QStringLiteral("SimAgv %1: 到达站点 %2")
                                        .arg(agvNumber)
                                        .arg(QString::fromStdString(targetStationId));
                m_logger->debug(msg);
            }

            state->lastStation = targetStation;
            // 检查是否完成整条路径
            if (state->currentSegmentIndex >= state->pathStations.size()) {
                state->velocity = 0.0;
                state->acceleration = 0.0;
                state->pathStations.clear();
                state->currentSegmentIndex = 0;

                if (m_logger) {
                    m_logger->info(QStringLiteral("SimAgv %1: 完成路径").arg(agvNumber));
                }
            }
        } else {
            // 简化物理模型：恒速运动
            double moveDistance = std::min(m_maxSpeed * deltaTime, distance);
            QPointF normalized = direction / distance;
            state->position += normalized * moveDistance;
            state->heading = std::atan2(normalized.y(), normalized.x());
            state->velocity = m_maxSpeed;
            state->acceleration = 0.0;
        }
    }

    // 更新电量：简单线性模型
    constexpr double kMoveDrainPerSec = 0.06;   // 行驶耗电速度: 0.06%/s
    constexpr double kIdleDrainPerSec = 0.02;   // 静止耗电速度: 0.02%/s
    constexpr double kChargeGainPerSec = 0.6;   // 充电回升速度: 0.6%/s

    const bool moving = !paused && state->velocity > 0.01;
    double batteryDelta = 0.0;
    if (state->charging) {
        batteryDelta = kChargeGainPerSec * deltaTime;
    } else if (moving) {
        batteryDelta = -kMoveDrainPerSec * deltaTime;
    } else {
        batteryDelta = -kIdleDrainPerSec * deltaTime;
    }

    state->batteryLevelPercent = std::clamp(state->batteryLevelPercent + batteryDelta, 0.0, 100.0);

}

PhysicsSimulator::VehicleState PhysicsSimulator::getState(int agvNumber) const {
    return m_vehicles.value(agvNumber);
}

void PhysicsSimulator::setPath(int agvNumber, const std::vector<std::string>& stations) {
    auto* state = getVehicleState(agvNumber);
    if (!state) {
        if (m_logger) {
            m_logger->warning(QStringLiteral("SimAgv %1: 车辆不存在，无法设置路径").arg(agvNumber));
        }
        return;
    }

    state->pathStations = stations;
    state->currentSegmentIndex = 0;
    state->charging = false;

    if (m_logger) {
        QString pathStr;
        for (size_t i = 0; i < stations.size(); ++i) {
            if (i > 0) pathStr += QStringLiteral(" -> ");
            pathStr += QString::fromStdString(stations[i]);
        }
        const QString msg =
            QStringLiteral("SimAgv %1: 设置路径: %2").arg(agvNumber).arg(pathStr);
        m_logger->info(msg);
    }
}

void PhysicsSimulator::appendPath(int agvNumber, const std::vector<std::string>& stations) {
    auto* state = getVehicleState(agvNumber);
    if (!state) {
        return;
    }

    state->pathStations.insert(state->pathStations.end(), stations.begin(), stations.end());

    if (m_logger) {
        const QString msg =
            QStringLiteral("SimAgv %1: 追加路径段，共 %2 个站点")
                .arg(agvNumber)
                .arg(state->pathStations.size());
        m_logger->debug(msg);
    }
}

void PhysicsSimulator::pauseVehicle(int agvNumber) {
    auto* state = getVehicleState(agvNumber);
    if (!state) {
        return;
    }

    state->isPaused = true;
    state->velocity = 0.0;

    if (m_logger) {
        m_logger->info(QStringLiteral("SimAgv %1: 暂停").arg(agvNumber));
    }
}

void PhysicsSimulator::resumeVehicle(int agvNumber) {
    auto* state = getVehicleState(agvNumber);
    if (!state) {
        return;
    }

    state->isPaused = false;

    if (m_logger) {
        m_logger->info(QStringLiteral("SimAgv %1: 恢复").arg(agvNumber));
    }
}

void PhysicsSimulator::cancelVehicle(int agvNumber) {
    auto* state = getVehicleState(agvNumber);
    if (!state) {
        return;
    }

    state->pathStations.clear();
    state->currentSegmentIndex = 0;
    state->velocity = 0.0;
    state->acceleration = 0.0;
    state->charging = false;

    if (m_logger) {
        m_logger->info(QStringLiteral("SimAgv %1: 取消任务").arg(agvNumber));
    }
}

void PhysicsSimulator::registerVehicle(int agvNumber,
                                       const std::string& initialStation,
                                       const std::string& mapName,
                                       double initialBatteryPercent) {
    VehicleState state;
    state.agvNumber = agvNumber;
    state.currentMap = QString::fromStdString(mapName);
    state.lastStation = QString::fromStdString(initialStation);
    state.batteryLevelPercent = std::clamp(initialBatteryPercent, 0.0, 100.0);
    state.velocity = 0.0;
    state.acceleration = 0.0;
    state.charging = false;

    // 获取初始位置
    QString stationId = QString::fromStdString(initialStation);
    state.position = getStationPosition(stationId, state.currentMap);

    m_vehicles.insert(agvNumber, state);

    if (m_logger) {
        const QString msg = QStringLiteral("SimAgv %1: 注册成功，初始站点 %2, 地图 %3")
                                .arg(agvNumber)
                                .arg(QString::fromStdString(initialStation))
                                .arg(QString::fromStdString(mapName));
        m_logger->info(msg);
    }
}

void PhysicsSimulator::unregisterVehicle(int agvNumber) {
    m_vehicles.remove(agvNumber);

    if (m_logger) {
        m_logger->info(QStringLiteral("SimAgv %1: 注销").arg(agvNumber));
    }
}

void PhysicsSimulator::setCharging(int agvNumber, bool charging) {
    auto* state = getVehicleState(agvNumber);
    if (!state) {
        return;
    }

    state->charging = charging;

    if (m_logger) {
        m_logger->info(QStringLiteral("SimAgv %1: %2充电模式")
                           .arg(agvNumber)
                           .arg(charging ? QStringLiteral("进入") : QStringLiteral("退出")));
    }
}

bool PhysicsSimulator::hasVehicle(int agvNumber) const {
    return m_vehicles.contains(agvNumber);
}

QPointF PhysicsSimulator::getStationPosition(const QString& stationId, const QString& mapName) {
    if (!m_mapService) {
        if (m_logger) {
            m_logger->warning(QStringLiteral("物理模拟器: MapService 未初始化"));
        }
        return QPointF(0, 0);
    }

    // 从地图名称获取楼层号
    int floor = m_mapService->getFloorForMapName(mapName);
    if (floor == -1) {
        if (m_logger) {
            m_logger->warning(QStringLiteral("物理模拟器: 无法找到地图 %1 对应的楼层").arg(mapName));
        }
        return QPointF(0, 0);
    }

    // 从 MapApplicationService 获取真实站点坐标
    kernel::Position position = m_mapService->getStationPosition(stationId, floor);

    if (m_logger) {
        m_logger->debug(QStringLiteral("物理模拟器: 站点 %1 (地图:%2, 楼层:%3) 坐标 (%4, %5)")
                            .arg(stationId)
                            .arg(mapName)
                            .arg(floor)
                            .arg(position.x, 0, 'f', 2)
                            .arg(position.y, 0, 'f', 2));
    }

    return QPointF(position.x, position.y);
}

PhysicsSimulator::VehicleState* PhysicsSimulator::getVehicleState(int agvNumber) {
    if (!m_vehicles.contains(agvNumber)) {
        return nullptr;
    }
    return &m_vehicles[agvNumber];
}

} // namespace simulation
} // namespace application
